'''
Created on Aug 5, 2019

@author: modys
'''
import numpy as np
class Bicycle():
    '''
    classdocs
    '''

    def __init__(self):
        '''
        Constructor
        '''
        self.xc = 0;
        self.yc = 0;
        self.theta = 0;
        self.delta = 0;
        self.beta = 0;
        
        self.L = 2;
        self.lr = 1.2;
        self.w_max = 1.22;
        
        self.sample_time = 0.01   ;
        
    def reset(self):
        self.xc = 0;
        self.yc = 0;
        self.theta = 0;
        self.delta = 0;
        self.beta = 0;
        
    def step(self, v, w):
        max(-self.w_max, min(w, self.w_max))
        self.xc    = self.xc + v*np.cos(self.theta +self.beta)*self.sample_time;
        self.yc    = self.yc + v*np.sin(self.theta +self.beta)*self.sample_time;
        self.theta = self.theta + (v*np.cos(self.beta)*np.tan(self.delta)/self.L)*self.sample_time;
        self.delta = self.delta + w * self.sample_time;
        self.beta  = np.arctan((self.lr*np.tan(self.delta))/self.L);       